from time import sleep
from maix import camera, display, image, nn, app, uart,pinmap,time
import struct
import sys
sys.path.append('/root/exam')
import serial_protocol 

detector1 = nn.YOLOv5(model="/root/thing/model_175499.mud", dual_buff = True)   #物料探测
detector2 = nn.YOLOv5(model="/root/place/model_166546.mud", dual_buff = True)   #标靶探测
comm_proto = serial_protocol.SerialProtocol()

cam = camera.Camera(detector1.input_width(), detector1.input_height(), detector1.input_format())
cam.luma(45)
cam.constrast(100)
cam.saturation(60)
dis = display.Display()

index = {'1':'red','2':'green','3':'blue'}
yolo1_index = {'1':1,'2':0,'3':2}
sehuan_index = [2,3,1]
wuliao_index = [2,1,3]
thing_location = [150,122]#物料位置设定坐标
place_location = [143,95]#放置位置设定坐标
# keypoints = [150, 122, 143, 95]#关键点坐标
location = [0,0]
ex_location = [0,0]
obj_count = 0
still_count = 3
position1 = False    #物料定位标志
position2 = True     #色环定位标志
position3 = False    #物料定位标志 
threshold1 = 7500 
threshold2 = 7000   
threshold3 = 5300    
#bits = struct.pack('<i',0)

# paylocations = [location[0],location[0],location[0] * -1,location[1] * -1]
# encoded_locations = [comm_proto.encode(struct.pack('<i', loc)) for loc in paylocations]
colornumber=struct.pack('<i',wuliao_index[0])
encodedcolor = comm_proto.encode(colornumber)
paylocation0 = struct.pack('<i',location[0])
paylocation2 = struct.pack('<i',location[0]*-1)
paylocation1 = struct.pack('<i',location[1])
paylocation3 = struct.pack('<i',location[1]*-1)
encodedloc0 = comm_proto.encode(paylocation0)
encodedloc1 = comm_proto.encode(paylocation1)
encodedloc2 = comm_proto.encode(paylocation2)
encodedloc3 = comm_proto.encode(paylocation3)


def base_received(serial : uart.UART, data : bytes):    #设定只有在收到请求后才会发出数据
    global position1,position2,position3
    global threshold1,threshold2,threshold3
    print("received:", data)
    
    if data == b'a':
        position2 = False
        threshold2 = 7000
        position1 = True

    elif data == b'b':
        position1 = False
        threshold1 = 7500   

    elif data == b'c':
        position1 = False 
        threshold1 = 7500 
        position2 = True
        
    elif data == b'd':
        position2 = False 
        threshold2 = 7000

    elif data == b'e':
        position2 = False
        threshold2 = 7000
        position3 = True 

    elif data == b'f':
        position3 = False
        threshold3 = 5300
    

ports = uart.list_devices()
# pinmap.set_pin_function("A19", "UART1_TX")
# pinmap.set_pin_function("A18", "UART1_RX")
# device1 = "/dev/ttyS1"
# serial1 = uart.UART(device1, 115200)
# serial1.set_received_callback(base_received)
pinmap.set_pin_function("A28", "UART2_TX")
pinmap.set_pin_function("A29", "UART2_RX")
device2 = "/dev/ttyS2"
serial2 = uart.UART(device2, 115200)
serial2.set_received_callback(base_received)
        
def uart_transmit_l(loc : list[int], locdata0 : bytes, locdata1 : bytes, locdata2 : bytes, locdata3 : bytes, colordata : bytes):
    global serial2
    if loc[0] > 0:
        serial2.write(b'\0')
        serial2.write(locdata1)
    else:
        serial2.write(locdata3)
        serial2.write(b'\0')

    if loc[1] > 0:
        serial2.write(locdata0)
        serial2.write(b'\0')  
    else: 
        serial2.write(b'\0')
        serial2.write(locdata2)     
    
    serial2.write(colordata)
    serial2.write(b'\0')
    serial2.write(b'\0')
    serial2.write(b'\0')

def uart_transmit_f(loc : list[int], locdata0 : bytes, locdata1 : bytes, locdata2 : bytes, locdata3 : bytes, colordata : bytes):
    global serial2
    if location[1] > 0:
        serial2.write(b'\0')
        serial2.write(locdata0)
    else: 
        serial2.write(locdata2)
        serial2.write(b'\0')
    if location[0] > 0:
        serial2.write(b'\0')
        serial2.write(locdata1)
    else:
        serial2.write(locdata3)
        serial2.write(b'\0')
    serial2.write(colordata)
    serial2.write(b'\0')
    serial2.write(b'\0')
    serial2.write(b'\0')  
       
while not app.need_exit():
    img = cam.read()
    
    if position1:#物料定位阶段   
        objs1 = detector1.detect(img, conf_th = 0.4, iou_th = 0.45)   
        for obj in objs1:
            threshold1-=50
            img.draw_string(0, 185, "resource", image.COLOR_GREEN)
            roi_list = [obj.x-10, obj.y-10, obj.w+20, obj.h+20]
            circles = img.find_circles(roi = roi_list, threshold = threshold1, r_min = 20)
            if circles:
                largest_cirle = max(circles, key=lambda c: c.r())
                img.draw_circle(largest_cirle.x(), largest_cirle.y(), largest_cirle.r(), image.COLOR_RED, 2)
                img.draw_cross(largest_cirle.x(),largest_cirle.y(),image.COLOR_GREEN, 2)
                img.draw_keypoints([150, 122], image.COLOR_GREEN, 10, 1)
                
                location[0] = location[0]+(largest_cirle.x()-thing_location[0])
                location[1] = location[1]+(largest_cirle.y()-thing_location[1])
                obj_count+=1
                threshold1+=60
            print(wuliao_index[obj.class_id])
            print(threshold1)
            colornumber=struct.pack('<i',wuliao_index[obj.class_id])
            encodedcolor = comm_proto.encode(colornumber)
        if obj_count!=0 and (location[0]<40*obj_count or location[1]<40*obj_count):
            location[0]//=obj_count*5
            location[1]//=obj_count*5
            if still_count == 3:
                ex_location[0]=location[0]
                ex_location[1]=location[1]
                still_count-=1
            elif still_count == 0:
                print(location[0],location[1])

                paylocation1 = struct.pack('<i',location[0])#x轴为前后的坐标
                paylocation3 = struct.pack('<i',location[0]*-1)
                paylocation0 = struct.pack('<i',location[1])#y轴为左右的坐标
                paylocation2 = struct.pack('<i',location[1]*-1)

                encodedloc0 = comm_proto.encode(paylocation0)
                encodedloc1 = comm_proto.encode(paylocation1)
                encodedloc2 = comm_proto.encode(paylocation2)
                encodedloc3 = comm_proto.encode(paylocation3)
                
                uart_transmit_l(location,encodedloc0,encodedloc1,encodedloc2,encodedloc3,encodedcolor)
                # if location[0] > 0:
                #     serial2.write(b'\0')
                #     serial2.write(encodedloc1)
                # else:
                #     serial2.write(encodedloc3)
                #     serial2.write(b'\0')

                # if location[1] > 0:
                #     serial2.write(encodedloc0)  #encoded_locations[0]
                #     serial2.write(b'\0')  
                # else: 
                #     serial2.write(b'\0')
                #     serial2.write(encodedloc2)     
                
                # serial2.write(encodedcolor)
                # serial2.write(b'\0')
                # serial2.write(b'\0')
                # serial2.write(b'\0')  
                still_count = 3
                ex_location = [0,0] 
            elif location[0]-ex_location[0]<10 and location[1]-ex_location[1]<10:
                still_count-=1
            else:
                still_count = 3
            location = [0,0]
            obj_count=0
            
    elif position2: #第一次粗加工区，1暂存区，2粗加工区定位阶段 
        objs2 = detector2.detect(img, conf_th = 0.55, iou_th = 0.45)  
        for obj in objs2:
            threshold2-=50
            img.draw_string(0, 185, "process", image.COLOR_GREEN)
            roi_list = [obj.x-5, obj.y-5, obj.w+10, obj.h+10]
            circles = img.find_circles(roi = roi_list, x_stride = 1,  threshold = threshold2, r_min = 20, r_step = 1)
            if circles:
                largest_cirle = max(circles, key=lambda c: c.r())
                img.draw_circle(largest_cirle.x(), largest_cirle.y(), largest_cirle.r(), image.COLOR_RED, 2)
                img.draw_cross(largest_cirle.x(),largest_cirle.y(),image.COLOR_GREEN, 2)
                img.draw_keypoints([143, 95], image.COLOR_GREEN, 10, 1)

                location[0] = location[0]+(largest_cirle.x()-place_location[0])
                location[1] = location[1]+(largest_cirle.y()-place_location[1])
                obj_count+=1
                threshold2+=60
            print(wuliao_index[obj.class_id])
            print(threshold2)
            colornumber=struct.pack('<i',sehuan_index[obj.class_id])
            encodedcolor = comm_proto.encode(colornumber)
        if obj_count!=0:
            location[0]//=obj_count
            location[1]//=obj_count
            print(location[0],location[1])
            
            paylocation0 = struct.pack('<i',location[1])#y轴为前后的坐标
            paylocation2 = struct.pack('<i',location[1]*-1)
            paylocation1 = struct.pack('<i',location[0])#x轴为左右的坐标
            paylocation3 = struct.pack('<i',location[0]*-1)
            encodedloc0 = comm_proto.encode(paylocation0)
            encodedloc1 = comm_proto.encode(paylocation1)
            encodedloc2 = comm_proto.encode(paylocation2)
            encodedloc3 = comm_proto.encode(paylocation3)

            uart_transmit_f(location,encodedloc0,encodedloc1,encodedloc2,encodedloc3,encodedcolor)
            # if location[1] > 0:
            #     serial2.write(b'\0')
            #     serial2.write(encodedloc0)  #encoded_locations[0]
            # else: 
            #     serial2.write(encodedloc2)
            #     serial2.write(b'\0')
            # if location[0] > 0:
            #     serial2.write(b'\0')
            #     serial2.write(encodedloc1)
            # else:
            #     serial2.write(encodedloc3)
            #     serial2.write(b'\0')
            
            # serial2.write(encodedcolor)
            # serial2.write(b'\0')
            # serial2.write(b'\0')
            # serial2.write(b'\0')  
            location = [0,0] 
            obj_count=0

    elif position3: #第二次暂存区定位阶段 
        objs3 = detector1.detect(img, conf_th = 0.4, iou_th = 0.45) 
        img.draw_string(0, 185, "last save", image.COLOR_GREEN) 
        for obj in objs3:
            threshold3-=50
            roi_list = [obj.x, obj.y, obj.w, obj.h]
            circles = img.find_circles(roi = roi_list, x_stride = 1, threshold = threshold3, r_min = 20, r_step = 1)
            if circles:
                largest_cirle = max(circles, key=lambda c: c.r())
                img.draw_circle(largest_cirle.x(), largest_cirle.y(), largest_cirle.r(), image.COLOR_RED, 2)
                img.draw_cross(largest_cirle.x(),largest_cirle.y(),image.COLOR_GREEN, 2)

                location[0] = location[0]+(largest_cirle.x()-place_location[0])
                location[1] = location[1]+(largest_cirle.y()-place_location[1])
                obj_count+=1
                threshold3+=60
            print(wuliao_index[obj.class_id])
            print(threshold3)
            colornumber=struct.pack('<i',wuliao_index[obj.class_id])
            encodedcolor = comm_proto.encode(colornumber)
        if obj_count!=0:
            location[0]//=obj_count*3
            location[1]//=obj_count*3
            print(location[0],location[1])
            
            paylocation0 = struct.pack('<i',location[1])#y轴为前后的坐标
            paylocation2 = struct.pack('<i',location[1]*-1)
            paylocation1 = struct.pack('<i',location[0])#x轴为左右的坐标
            paylocation3 = struct.pack('<i',location[0]*-1)
            encodedloc0 = comm_proto.encode(paylocation0)
            encodedloc1 = comm_proto.encode(paylocation1)
            encodedloc2 = comm_proto.encode(paylocation2)
            encodedloc3 = comm_proto.encode(paylocation3)

            uart_transmit_f(location,encodedloc0,encodedloc1,encodedloc2,encodedloc3,encodedcolor)
            # if location[1] > 0:
            #     serial2.write(b'\0')
            #     serial2.write(encodedloc0)
            # else: 
            #     serial2.write(encodedloc2)
            #     serial2.write(b'\0')
            # if location[0] > 0:
            #     serial2.write(b'\0')
            #     serial2.write(encodedloc1)
            # else:
            #     serial2.write(encodedloc3)
            #     serial2.write(b'\0')
            
            # serial2.write(encodedcolor)
            # serial2.write(b'\0')
            # serial2.write(b'\0')
            # serial2.write(b'\0')  
            location = [0,0] 
            obj_count=0   
            
    dis.show(img)


    
# from init_pinversion import pin_init1,pin_init2,TaskStatus

# import vision
# err_count = 0
# proces_location= [108,109]#物料位置设定坐标
# command1 = "123"    #搬运命令存储变量
# command2 = "123"
# command = command1 +command2 + "\n"

# vision = vision.Location() #Location=[0,0],place_location = [125,101]
# task_status = TaskStatus()

# 更新状态
# task_status.qr_flag = False
# task_status.position = False
# task_status.target = False
# task_status.grabcommand = False

# qr_flag = True     #二维码获取标志
# position = False    #定位完成标志
# target = False    #目标检测完成标志
# grabcommand = False #物料未识别完成标志
# inde = 0               #抓取物料的序号
# times = 10           #任务流程序号
# paylocation0 = struct.pack('<i',location[0])
# paylocation2 = struct.pack('<i',location[0]*-1)
# paylocation1 = struct.pack('<i',location[0])
# paylocation3 = struct.pack('<i',location[1]*-1)
# encodedloc0 = comm_proto.encode(paylocation0)
# encodedloc1 = comm_proto.encode(paylocation1)
# encodedloc2 = comm_proto.encode(paylocation2)
# encodedloc3 = comm_proto.encode(paylocation3)


# base_received()
# stm32_received()
# answerflag = False

# def stm32_received(serial : uart.UART, data : bytes):
#     print("received:", data) 
#     global grabcommand,formationflag
#     if data:
#         formationflag = True
#         if data == b'a':
#             if grabcommand:
#                 grabcommand = False 
#                 print('sent: y')
#                 serial2.write(b'y')
#                 sleep(10)
    

# serial1 = pin_init1()  
# serial2 = pin_init2()   


# serial2.write_str("hello world\n")
        
        # if not task_status.qr_flag:
        #     string = vision.qrcodes_detect()
        #     print("string:",string)
        #     # serial2.write_str(string)
        #     # qrcodes = img.find_qrcodes()
        #     # for q in qrcodes:
        #     #     corners = q.corners()
        #     #     for i in range(4):
        #     #         img.draw_line(corners[i][0], corners[i][1], corners[(i + 1) % 4][0], corners[(i + 1) % 4][1], image.COLOR_RED)
        #     #     string = q.payload()
        #     #     serial2.write_str(string)
        #     if (string!='0'):
        #         command1 = string[0:3]
        #         command2 = string[4:7]

        #         command = command1 +command2 + "\n"
        #         serial1.write(command.encode())
                    
        #         print('('+index[command1[0]]+'+'+index[command1[1]]+'+'+index[command1[2]]+'&'+index[command2[0]]+'+'+index[command2[1]]+'+'+index[command2[2]]+')')
        #     task_status.qr_flag = True
        #     task_status.times = 10      #结束进入下一流程
                # while not answerflag:
                #     serial1.write(command.encode())

                #     # serial1.write_str(command)
                #     sleep(1)
        # else:
        #     img.draw_string(0, 0, index[command1[0]]+'+'+index[command1[1]]+'+'+index[command1[2]]+'\n'+index[command2[0]]+'+'+index[command2[1]]+'+'+index[command2[2]], image.COLOR_BLUE)
        # if task_status.times == 10 or task_status.times == 20:         #第一/二次拿物料
            # if task_status.times == 10:
                # img.draw_string(0, 210, "status:get first time", image.COLOR_RED)
                # print( "status:get first time")
            # else:
                # print( "status:get second time")
                # img.draw_string(0, 210, "status:get second time", image.COLOR_RED)
            # objs = vision.detector1.detect(img, conf_th = 0.4, iou_th = 0.45)
            # objs = vision.objs1
            
            # for obj in objs:
        # img = vision.cam.read()  
       

            # else:       #抓取阶段
            #     # img.draw_string(0, 185, "base:done", image.COLOR_GREEN)
            #     # if task_status.times == 10:
                    # if obj.class_id == yolo1_index[command1[task_status.inde]]:  #顺序判断
            #             print("grab1!")
            #             task_status.inde+=1
            #             grabcommand = True
            #             sleep(1)
            #     # else:
            #         if obj.class_id == yolo1_index[command2[task_status.inde]]:  #顺序判断
            #             print("grab2!")
            #             task_status.inde+=1
            #             grabcommand = True
            #             sleep(1)
                
            # if task_status.inde == 3:
                # task_status.inde = 0
                # task_status.times = task_status.times + 1       #结束进入下一流程
                # position1 = False
                # task_status.movement = False
                # location = [0,0] 
                # obj_count=0    
                # task_status.target = False 

        # elif task_status.times == 11 or task_status.times ==21:       #第一/二次粗加工
            # if task_status.times == 11:
            #     print("status:process first time")
            #     # img.draw_string(0, 210, "status:process first time", image.COLOR_RED)
            # else:
            #     print("status:process second time")
                # img.draw_string(0, 210, "status:proces second time", image.COLOR_RED)
            # objs = detector2.detect(img, conf_th = 0.4, iou_th = 0.45)
            # for obj in objs:
            #     if not position:#定位阶段
            #         roi_list = [obj.x-25, obj.y-25, obj.w+50, obj.h+50]
            #         circles = img.find_circles(roi = roi_list, threshold = 4000)
            #         for a in circles:
            #             if(a.r()<30):
            #                 continue
            #             img.draw_circle(a.x(), a.y(), a.r(), image.COLOR_RED, 2)
        


        # elif task_status.times == 12 or task_status.times ==22:       #第一/二次存放
            # if task_status.times == 12:
            #     print("status:save first time")
            #     # img.draw_string(0, 210, "status:save first time", image.COLOR_RED)
            # else:
            #     print("status:save second time")
                # img.draw_string(0, 210, "status:save second time", image.COLOR_RED)
            # objs = detector2.detect(img, conf_th = 0.4, iou_th = 0.45)
            # for obj in objs:
            #     if not position:#定位阶段
            #         roi_list = [obj.x-25, obj.y-25, obj.w+50, obj.h+50]
            #         circles = img.find_circles(roi = roi_list, threshold = 4000)
            #         for a in circles:
            #             if(a.r()<30):
            #                 continue
            #             img.draw_circle(a.x(), a.y(), a.r(), image.COLOR_RED, 2)
        #     if not task_status.position:   
        #         vision.draw_circles2(img)
        #         location = vision.get_placelocation()
                
        #         task_status.target = True
        #         # location[0] = location[0]+(a.x()-place_location[0])
        #         # location[1] = location[1]+(a.y()-place_location[1])
        #         obj_count+=1
        #         if obj_count!=0:
        #             location[0]//=obj_count*10
        #             location[1]//=obj_count*10
        #             print(location[0],location[1])
        #             paylocations = [location[0],location[0],location[0] * -1,location[1] * -1]
        #             encoded_locations = [comm_proto.encode(struct.pack('<i', loc)) for loc in paylocations]
        #         location = [0,0] 
        #         obj_count=0    
        #         task_status.target = False    
                    
        # else:
        #     # task_status.times = 20       #结束进入下一流程
        #     task_status.position = False
        #     task_status.movement = False
        #     location = [0,0] 
        #     obj_count=0    
        #     task_status.target = False 
    # dis.show(img)
    
        # elif times == 20:       #第二次拿物料
        #     img.draw_string(0, 210, "status:get second time", image.COLOR_RED)
        #     objs = detector1.detect(img, conf_th = 0.4, iou_th = 0.45)
        #     for obj in objs:
        #         if not position:#定位阶段
        #             img.draw_string(0, 185, "base:done", image.COLOR_GREEN)
        #             roi_list = [obj.x-20, obj.y-20, obj.w+40, obj.h+40]
        #             circles = img.find_circles(roi = roi_list, threshold = 5000)
        #             for a in circles:
        #                 if(a.r()<75):
        #                     continue
        #                 img.draw_circle(a.x(), a.y(), a.r(), image.COLOR_RED, 2)
        #                 target = True
        #                 location[0] = location[0]+(a.x()-thing_location[0])
        #                 location[1] = location[1]+(a.y()-thing_location[1])
        #                 obj_count+=1
        #             if obj_count!=0:
        #                 location[0]//=obj_count*10
        #                 location[1]//=obj_count*10
        #                 print(location[0],location[1])
        #                 paylocations = [location[0],location[0],location[0] * -1,location[1] * -1]
        #                 encoded_locations = [comm_proto.encode(struct.pack('<i', loc)) for loc in paylocations]
        #                 # paylocation0 = struct.pack('<i',location[0])
        #                 # paylocation2 = struct.pack('<i',location[0]*-1)
        #                 # paylocation1 = struct.pack('<i',location[0])
        #                 # paylocation3 = struct.pack('<i',location[1]*-1)
        #                 # encodedloc0 = comm_proto.encode(paylocation0)
        #                 # encodedloc1 = comm_proto.encode(paylocation1)
        #                 # encodedloc2 = comm_proto.encode(paylocation2)
        #                 # encodedloc3 = comm_proto.encode(paylocation3)
        #             location = [0,0] 
        #             obj_count=0    
        #             target = False

        #         else:       #抓取阶段
        #             if obj.class_id == index[command2[inde]]:  #顺序判断
        #                     inde+=1
        #                     grabcommand = True
        #     if inde == 3:
        #         inde = 0
        #         times = 21       #结束进入下一流程
        #         position = False
        #         movement = False
        #         location = [0,0]

        # elif times == 21:       #第二次粗加工
        #     img.draw_string(0, 210, "status:proces second time", image.COLOR_RED)
        #     objs = detector2.detect(img, conf_th = 0.4, iou_th = 0.45)
        #     for obj in objs:
        #         if not position:#定位阶段
        #             roi_list = [obj.x-25, obj.y-25, obj.w+50, obj.h+50]
        #             circles = img.find_circles(roi = roi_list, threshold = 4000)
        #             for a in circles:
        #                 if(a.r()<30):
        #                     continue
        #                 img.draw_circle(a.x(), a.y(), a.r(), image.COLOR_RED, 2)
        #                 location[0] = location[0]//2+(a.x()-place_location[0])//2
        #                 location[1] = location[1]//2+(a.y()-place_location[1])//2
        #                 print(location[0],location[1])
        #                 target = True
        #                 paylocations = [location[0],location[0],location[0] * -1,location[1] * -1]
        #                 encoded_locations = [comm_proto.encode(struct.pack('<i', loc)) for loc in paylocations]
        #                 # paylocation0 = struct.pack('<i',location[0])
        #                 # paylocation2 = struct.pack('<i',location[0]*-1)
        #                 # paylocation1 = struct.pack('<i',location[0])
        #                 # paylocation3 = struct.pack('<i',location[1]*-1)
        #                 # encodedloc0 = comm_proto.encode(paylocation0)
        #                 # encodedloc1 = comm_proto.encode(paylocation1)
        #                 # encodedloc2 = comm_proto.encode(paylocation2)
        #                 # encodedloc3 = comm_proto.encode(paylocation3)
                            
        #         else:       #结束进入下一流程
        #             times = 22
        #             position = False
        #             movement = False
        #             location = [0,0]

        # elif times == 22:       #第二次存放
        #     img.draw_string(0, 210, "status:save second time", image.COLOR_RED)
        #     objs = detector1.detect(img, conf_th = 0.4, iou_th = 0.45)
        #     for obj in objs:
        #         if not position:#定位阶段
        #             roi_list = [obj.x-25, obj.y-25, obj.w+50, obj.h+50]
        #             circles = img.find_circles(roi = roi_list, threshold = 4000)
        #             for a in circles:
        #                 if(a.r()<30):
        #                     continue
        #                 img.draw_circle(a.x(), a.y(), a.r(), image.COLOR_RED, 2)
        #                 location[0] = location[0]//2+(a.x()-thing_location[0])//2
        #                 location[1] = location[1]//2+(a.y()-thing_location[1])//2
        #                 print(location[0],location[1])
        #                 target = True
        #                 paylocations = [location[0],location[0],location[0] * -1,location[1] * -1]
        #                 encoded_locations = [comm_proto.encode(struct.pack('<i', loc)) for loc in paylocations]
        #                 # paylocation0 = struct.pack('<i',location[0])
        #                 # paylocation2 = struct.pack('<i',location[0]*-1)
        #                 # paylocation1 = struct.pack('<i',location[0])
        #                 # paylocation3 = struct.pack('<i',location[1]*-1)
        #                 # encodedloc0 = comm_proto.encode(paylocation0)
        #                 # encodedloc1 = comm_proto.encode(paylocation1)
        #                 # encodedloc2 = comm_proto.encode(paylocation2)
        #                 # encodedloc3 = comm_proto.encode(paylocation3)
                            
        #         else:       #结束进入下一流程
        #             times = 30
        #             position = False
        #             movement = False
        #             location = [0,0]
# if __name__ == '__main__':
#     main()